package trafficmanager

import (
	"context"
	"errors"
	"fmt"
	"strings"
	"sync"

	"dev.ixmxm.top/rms/boot"
	"dev.ixmxm.top/rms/utils/tools"

	"dev.ixmxm.top/rms/codes"
	"dev.ixmxm.top/rms/model"
	dijkstranew "dev.ixmxm.top/rms/utils/dijkstra"
	"dev.ixmxm.top/rms/utils/log"
)

func newMultiMapPlaner(db *boot.MongoDB) *MultiMapPlaner {
	planer := &MultiMapPlaner{graph: dijkstranew.NewGraph(), mapGraph: dijkstranew.NewGraph(), db: db, planers: make(map[string]*dijkstranew.Graph, 0)}
	return planer
}

type MultiMapPlaner struct {
	db           *boot.MongoDB
	mapRelations []model.MapRelation
	graph        *dijkstranew.Graph
	mapGraph     *dijkstranew.Graph
	mapMap       map[string]*model.Map
	mapMapLocker sync.RWMutex
	planers      map[string]*dijkstranew.Graph
}

//initDijkstraMap 初始化全图，除了起点，代价地图
func (planer *MultiMapPlaner) initPlaner(mapList []*model.Map) {
	planer.mapMap = make(map[string]*model.Map)
	for _, mapInfo := range mapList {
		planer.mapMap[mapInfo.MapName] = mapInfo
		//创建代价地图
		graph := dijkstranew.NewGraph()
		for _, l := range mapInfo.MapData.Paths {
			if l.PathVel == 0 {
				l.PathVel = 800
			}
			cost := l.PathLength / l.PathVel
			s := l.NodeB.Id + ":" + l.NodeB.Id
			t := l.NodeA.Id + ":" + l.NodeA.Id
			if l.Dir == model.DirectionA2B {
				s = l.NodeA.Id + ":" + l.NodeA.Id
				t = l.NodeB.Id + ":" + l.NodeB.Id
			}
			graph.AddEdge(s, t, cost)
		}
		planer.planers[mapInfo.MapName] = graph
	}
}
func gotSameMapConnection(curMap string, mapRelations []model.MapRelation) []string {
	site_ids := make([]string, 0)
	for _, l := range mapRelations {
		if l.FromMap == curMap {
			site_ids = append(site_ids, l.StartId)
		} else if l.ToMap == curMap {
			site_ids = append(site_ids, l.GoalId)
		}
	}
	return site_ids
}
func gotRelationMaps(mapRelations []model.MapRelation) map[string][]string {
	maps := make(map[string][]string, 0)
	for _, l := range mapRelations {
		maps[l.FromMap] = []string{}
		maps[l.ToMap] = []string{}
	}
	return maps
}
func gotRelationMapSites(mapRelations []model.MapRelation) map[string][]string {
	maps := gotRelationMaps(mapRelations)
	for m, _ := range maps {
		sites := gotSameMapConnection(m, mapRelations)
		maps[m] = sites
	}
	return maps
}
func (planer *MultiMapPlaner) CrossMapCanReach(startMap, endMap string) bool {
	_, _, errPlan := planer.mapGraph.Shortest(startMap, endMap)
	if errPlan == nil {
		return true
	}
	return false
}

//initDijkstraMap 初始化全图
func (planer *MultiMapPlaner) initDijkstraMap(maps []*model.Map) {
	planer.mapMapLocker.Lock()
	defer planer.mapMapLocker.Unlock()
	planer.initPlaner(maps)
	graph := dijkstranew.NewGraph()

	GetBestGlobalPathCost := func(mapName, s, t string) (float64, error) {
		planner, isOk := planer.planers[mapName]
		if !isOk {
			log.ErrorfWithContext(context.Background(), "map %s not found", mapName)
			return 0, errors.New("map not found")
		}
		sourceStr := s + ":" + s
		targetStr := t + ":" + t
		_, cost, errPlan := planner.Shortest(sourceStr, targetStr)
		return cost, errPlan
	}
	mapGraph := dijkstranew.NewGraph()
	log.Infof("get map relations count %d", len(planer.mapRelations))
	for _, l := range planer.mapRelations {
		log.Infof("add relation %+v", l)

		s := l.StartId + ":" + l.FromMap
		t := l.GoalId + ":" + l.ToMap
		cost := float64(l.CostValue)
		graph.AddEdge(s, t, cost)

		ms := l.FromMap
		mt := l.ToMap
		mapGraph.AddEdge(ms, mt, cost)

		if l.IsTwoWay {
			graph.AddEdge(t, s, cost)
			mapGraph.AddEdge(mt, ms, cost)
		}
	}
	mapSites := gotRelationMapSites(planer.mapRelations)
	for m, sites := range mapSites {
		for index, site := range sites {
			for index1, site1 := range sites {
				if index1 != index && site != site1 {
					s := site + ":" + m
					t := site1 + ":" + m
					cost, errPlan := GetBestGlobalPathCost(m, site, site1)
					if errPlan != nil {
						continue
					}
					graph.AddEdge(s, t, cost)
				}
			}
		}
	}
	planer.graph = graph
	planer.mapGraph = mapGraph

	// planer.addTaskDijkstraMap(graph, "7B-1@0809", "17716", "7-1@0809", "17716")
	// sourceStr := fmt.Sprintf("%s:%s", "17719", "7B-1@0809")
	// targetStr := fmt.Sprintf("%s:%s", "10001", "7-3@333")
	// bestPath, cost, _ := planer.graph.Shortest(sourceStr, targetStr)
	// log.Infof("cost=%f  %v", cost, bestPath)
}

//initDijkstraMap 初始化全图
func (planer *MultiMapPlaner) addTaskDijkstraMap(graph *dijkstranew.Graph, fromMap, start, toMap, target string) {
	graph.RemoveOneVertex("17719:7-1@0809")
	graph.RemoveOneVertex("17719:7B-1@0809")
	graph.RemoveVertex("7B-1@0809")
	graph.RemoveVertex("7-1@0809")
	graph.RemoveEdge("7B-1@0809", "7-1@0809")
	graph.RemoveEdge("7-1@0809", "7B-1@0809")
	graph.RemoveOneEdge("17719:7B-1@0809", "17719:7-1@0809")
	gotSameMapConnection := func(curMap string, mapRelations []model.MapRelation) []string {
		site_ids := make([]string, 0)
		for _, l := range mapRelations {
			if l.FromMap == curMap {
				site_ids = append(site_ids, l.StartId)
			} else if l.ToMap == curMap {
				site_ids = append(site_ids, l.GoalId)
			}
		}
		return site_ids
	}
	GetBestGlobalPathCost := func(mapName, s, t string) float64 {
		sourceStr := fmt.Sprintf("%s:%s", s, s)
		targetStr := fmt.Sprintf("%s:%s", t, t)
		_, cost, _ := planer.planers[mapName].Shortest(sourceStr, targetStr)
		return cost
	}
	olds := planer.mapRelations
	sites := gotSameMapConnection(fromMap, olds)
	for _, site := range sites {
		s := fmt.Sprintf("%s:%s", start, fromMap)
		t := fmt.Sprintf("%s:%s", site, fromMap)
		cost := GetBestGlobalPathCost(fromMap, start, site)
		graph.AddEdge(s, t, cost)
		s = fmt.Sprintf("%s:%s", site, fromMap)
		t = fmt.Sprintf("%s:%s", start, fromMap)
		graph.AddEdge(s, t, cost)
	}
	sites = gotSameMapConnection(toMap, olds)
	for _, site := range sites {
		s := fmt.Sprintf("%s:%s", target, toMap)
		t := fmt.Sprintf("%s:%s", site, toMap)
		cost := GetBestGlobalPathCost(toMap, target, site)
		graph.AddEdge(s, t, cost)
		s = fmt.Sprintf("%s:%s", site, toMap)
		t = fmt.Sprintf("%s:%s", target, toMap)
		graph.AddEdge(s, t, cost)
	}

	sourceStr := fmt.Sprintf("%s:%s", start, fromMap)
	targetStr := fmt.Sprintf("%s:%s", target, toMap)
	bestPath, cost, _ := graph.Shortest(sourceStr, targetStr)
	log.Infof("cost=%f  %v", cost, bestPath)

	findMapInfo := func(fromMap, s, toMap, t string) model.TaskInfo {
		task_info := model.TaskInfo{}
		for _, l := range planer.mapRelations {
			// if fromMap == l.FromMap && toMap == l.ToMap {
			if fromMap == l.FromMap && toMap == l.ToMap && s == l.StartId && t == l.GoalId {
				task_info.TaskMapName = l.FromMap
				task_info.TaskSiteId = l.StartId
				task_info.ToMapName = l.ToMap
				task_info.ToMapSite = l.GoalId
				task_info.ToMapPath = l.GoalPathId
				task_info.DeviceCode = l.DeviceCode
				task_info.SwitchType = l.Type
				return task_info
				// } else if fromMap == l.ToMap && toMap == l.FromMap {
			} else if fromMap == l.ToMap && toMap == l.FromMap && s == l.GoalId && t == l.StartId {
				task_info.TaskMapName = l.ToMap
				task_info.TaskSiteId = l.GoalId
				task_info.ToMapName = l.FromMap
				task_info.ToMapSite = l.StartId
				task_info.ToMapPath = l.StartPathId
				task_info.DeviceCode = l.DeviceCode
				task_info.SwitchType = l.Type
				return task_info
			}
		}
		return task_info
	}
	task_infos := make([]model.TaskInfo, 0)
	for index, m := range bestPath {
		cur_id := strings.Split(m, ":")[0]
		cur_map := strings.Split(m, ":")[1]
		if index < len(bestPath)-1 {

			next := bestPath[index+1]
			next_id := strings.Split(next, ":")[0]
			next_map := strings.Split(next, ":")[1]

			task_info := findMapInfo(cur_map, cur_id, next_map, next_id)
			if task_info.TaskMapName == "" || task_info.ToMapName == "" {
				continue
			}
			task_infos = append(task_infos, task_info)
		}
	}
}

//根据车辆配置的地图集合，剔除无关路径
//电梯占用剔除相关路径
//电梯禁用AGV列表剔除无关路径
func (planer *MultiMapPlaner) makeFullDijkstraMap(ctx context.Context, robot *model.Robot, elevators map[string]*model.Elevator, sites []model.LockedSite) *dijkstranew.Graph {
	//拷贝代价地图
	graph := planer.graph.DeepCopy()
	removeMapPointEdge := func(l model.MapRelation) {
		log.Warnf("remove relation %+v", l)
		s := l.StartId + ":" + l.FromMap
		t := l.GoalId + ":" + l.ToMap
		graph.RemoveOneVertex(s)
		graph.RemoveOneVertex(t)
	}
	siteInLockList := func(map_name, id string) bool {
		for _, s := range sites {
			if s.MapName == map_name && s.SiteId == id {
				return true
			}
		}
		return false
	}
	robotInBanList := func(elevator *model.Elevator) bool {
		for _, banAgv := range elevator.BanAgvList {
			if banAgv == robot.Account.AgvId {
				return true
			}
		}
		return false
	}
	elevatorDisable := func(l model.MapRelation) bool {
		if l.Type == model.FloorRelation {
			if _, find := elevators[l.DeviceCode]; find {
				return false
			}
			return true
		}
		return false
	}
	for _, l := range planer.mapRelations {
		removeFlag := false
		elevator, find := elevators[l.DeviceCode]
		if find {
			if elevator.AgvId != "" || !elevator.IsEnable || !elevator.Online {
				if robot != nil && robot.Account != nil && robot.Account.AgvId != "" && robot.Account.AgvId == elevator.AgvId {
					removeFlag = false
				} else {
					removeMapPointEdge(l)
					removeFlag = true
				}
			} else {
				if robotInBanList(elevator) {
					removeMapPointEdge(l)
					removeFlag = true
				}
			}
		}
		if removeFlag {
			continue
		}
		if elevatorDisable(l) {
			removeMapPointEdge(l)
		}
		//获取厂区切换的任务，把反向的路径剔除
		if l.Type == model.FactoryRealtion && l.IsTwoWay && l.AgvId != "" {
			if l.PositiveWay {
				graph.RemoveOneEdge(fmt.Sprintf("%s:%s", l.GoalId, l.ToMap), fmt.Sprintf("%s:%s", l.StartId, l.FromMap))
			} else {
				graph.RemoveOneEdge(fmt.Sprintf("%s:%s", l.StartId, l.FromMap), fmt.Sprintf("%s:%s", l.GoalId, l.ToMap))
			}
		}
		if siteInLockList(l.FromMap, l.StartId) || siteInLockList(l.ToMap, l.GoalId) {
			graph.RemoveOneVertex(fmt.Sprintf("%s:%s", l.StartId, l.FromMap))
			graph.RemoveOneVertex(fmt.Sprintf("%s:%s", l.GoalId, l.ToMap))
		}
	}
	sameMapGraphs := make(map[string]*dijkstranew.Graph, 0)
	mapSites := gotRelationMapSites(planer.mapRelations)
	for m, sites := range mapSites {
		for index, site := range sites {
			for index1, site1 := range sites {
				if index1 != index && site != site1 {
					s := site + ":" + m
					t := site1 + ":" + m
					_, errPlan := planer.GetBestGlobalPathCost(sameMapGraphs, robot.Account.AgvId, m, site, site1)
					if errPlan != nil {
						graph.RemoveOneEdge(s, t)
					}
				}
			}
		}
	}
	return graph
}
func pathInBanArea(l model.Path, Areas map[string]model.Area, sn string) bool {
	for _, a := range Areas {
		if a.Type == model.AreaTypeBan && tools.StringSliceIsContain(a.BanAgvIds, sn) {
			if tools.StringSliceIsContain(a.ContainDotIds, l.NodeA.Id) || tools.StringSliceIsContain(a.ContainDotIds, l.NodeB.Id) {
				return true
			}
		}
	}
	return false
}
func (planer *MultiMapPlaner) GetBestGlobalPathCost(sameMapGraphs map[string]*dijkstranew.Graph, sn, mapName, s, t string) (float64, error) {
	sourceStr := s + ":" + s
	targetStr := t + ":" + t
	mapGraph, isOk := sameMapGraphs[mapName]
	if !isOk {
		if mapGraph, isOk = planer.planers[mapName]; !isOk {
			log.Infof("map planer %s not found", mapName)
			return 0, errors.New("map not found")
		}
		mapGraph = planer.planers[mapName].DeepCopy()

		if _, isOk = planer.mapMap[mapName]; !isOk {
			log.Infof("map data %s not found", mapName)
			return 0, errors.New("map not found")
		}
		mapData := planer.mapMap[mapName].MapData
		//剔除禁行区
		for _, l := range mapData.Paths {
			if !pathInBanArea(l, mapData.Areas, sn) {
				continue
			}

			if l.Dir == model.DirectionA2B {
				mapGraph.RemoveEdge(fmt.Sprint(l.NodeA.Id), fmt.Sprint(l.NodeB.Id))
			} else {
				mapGraph.RemoveEdge(fmt.Sprint(l.NodeB.Id), fmt.Sprint(l.NodeA.Id))
			}
		}
		sameMapGraphs[mapName] = mapGraph
		// planer.planers[mapName] = mapGraph
	}

	_, cost, errPlan := sameMapGraphs[mapName].Shortest(sourceStr, targetStr)
	// if errPlan != nil {
	// 	log.Errorf("panner error :%+v", errPlan)
	// }
	return cost, errPlan
}
func (planer *MultiMapPlaner) getCrossPath(ctx context.Context, robot *model.Robot, toMap, target string, elevators map[string]*model.Elevator, lockSites []model.LockedSite) ([]model.TaskInfo, float64, codes.Code) {
	planer.mapMapLocker.Lock()
	defer planer.mapMapLocker.Unlock()
	graphCopy := planer.makeFullDijkstraMap(ctx, robot, elevators, lockSites)
	sameMapGraphs := make(map[string]*dijkstranew.Graph, 0)
	findMapInfo := func(fromMap, s, toMap, t string) model.TaskInfo {
		task_info := model.TaskInfo{}
		for _, l := range planer.mapRelations {
			if fromMap == l.FromMap && toMap == l.ToMap && s == l.StartId && t == l.GoalId {
				task_info.TaskMapName = l.FromMap
				task_info.TaskSiteId = l.StartId
				task_info.ToMapName = l.ToMap
				task_info.ToMapSite = l.GoalId
				task_info.ToMapPath = l.GoalPathId
				task_info.DeviceCode = l.DeviceCode
				task_info.SwitchType = l.Type
				return task_info
			} else if fromMap == l.ToMap && toMap == l.FromMap && s == l.GoalId && t == l.StartId {
				task_info.TaskMapName = l.ToMap
				task_info.TaskSiteId = l.GoalId
				task_info.ToMapName = l.FromMap
				task_info.ToMapSite = l.StartId
				task_info.ToMapPath = l.StartPathId
				task_info.DeviceCode = l.DeviceCode
				task_info.SwitchType = l.Type
				return task_info
			}
		}
		return task_info
	}
	olds := planer.mapRelations
	fromMap := robot.Status.MapName
	start := robot.Status.SiteIdS
	sites := gotSameMapConnection(fromMap, olds)

	for _, site := range sites {
		s := start + ":" + fromMap
		t := site + ":" + fromMap
		cost, errPlan := planer.GetBestGlobalPathCost(sameMapGraphs, robot.Account.AgvId, fromMap, start, site)
		if errPlan != nil {
			continue
		}
		graphCopy.AddEdge(s, t, cost)
		graphCopy.AddEdge(t, s, cost)
	}
	sites = gotSameMapConnection(toMap, olds)
	for _, site := range sites {
		s := target + ":" + toMap
		t := site + ":" + toMap
		cost, errPlan := planer.GetBestGlobalPathCost(sameMapGraphs, robot.Account.AgvId, toMap, target, site)
		if errPlan != nil {
			continue
		}
		graphCopy.AddEdge(s, t, cost)
		graphCopy.AddEdge(t, s, cost)
	}

	sourceStr := start + ":" + fromMap
	targetStr := target + ":" + toMap
	bestPath, cost, _ := graphCopy.Shortest(sourceStr, targetStr)
	if len(bestPath) == 0 {
		return nil, 0, codes.ErrPathPlanError
	}

	task_infos := make([]model.TaskInfo, 0)
	for index, m := range bestPath {
		cur_id := strings.Split(m, ":")[0]
		cur_map := strings.Split(m, ":")[1]
		if index < len(bestPath)-1 {

			next := bestPath[index+1]
			next_id := strings.Split(next, ":")[0]
			next_map := strings.Split(next, ":")[1]

			task_info := findMapInfo(cur_map, cur_id, next_map, next_id)
			if task_info.TaskMapName == "" || task_info.ToMapName == "" {
				continue
			}
			task_infos = append(task_infos, task_info)
		}
	}
	if len(task_infos) == 0 {
		return nil, 0, codes.ErrPathPlanError
	}
	return task_infos, cost, nil
}
